#!/usr/bin/env python
#-*- coding: utf-8 -*-
#
# Copyright 2013 Antoine Drouin (poinix@gmail.com)
#
# This file is part of PAT.
#
#    PAT is free software: you can redistribute it and/or modify
#    it under the terms of the GNU General Public License as published by
#    the Free Software Foundation, either version 3 of the License, or
#    (at your option) any later version.
#
#    PAT is distributed in the hope that it will be useful,
#    but WITHOUT ANY WARRANTY; without even the implied warranty of
#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
#    GNU General Public License for more details.
#
#    You should have received a copy of the GNU General Public License
#    along with PAT.  If not, see <http://www.gnu.org/licenses/>.
#

import numpy as np
import scipy
import matplotlib.pyplot as plt
import sys

import pat.vehicles.fixed_wing.dynamic_model_python_basic as dm
import pat.vehicles.fixed_wing.control_3d as ctl
import pat.utils as pu

# get configuration files from command line arguments
if len(sys.argv) > 1:
    filenames = sys.argv[1:]
else:
    filenames = ["../config/Rcam_single_engine.xml"]

save_plots = False

# load parameters
dm_params = [dm.Param(f) for f in filenames]
for p in dm_params: print p

# finds trim conditions
va=dm_params[0].Vref; gamma=pu.rad_of_deg(0)
trims = [ctl.get_trim_cst_path(va, gamma, p, debug=True) for p in dm_params]

# numericaly integrate the ODE over time vector
# and plot trajectory
def run_sim(time, pert, title, filename=None):
    figure = None
    for i in range(0, len(dm_params)):
        Xe, Ue = trims[i]
        X0 = np.array(Xe) + pert
        X = scipy.integrate.odeint(dm.dyn, X0, time, args=(Ue, dm_params[i], ))
        figure = dm.plot_trajectory(time, X, figure, window_title=title)
    plt.legend(filenames, loc='best')
    if save_plots: plt.savefig(filename, dpi=80)


time = np.arange(0., 180., 0.01)

# runs an openloop simulation with a pitch perturbation
pert = np.zeros(dm.sv_size)
pert[dm.sv_theta] += pu.rad_of_deg(-10.)
sim_name = "Pitch perturbation: {:s}".format(filenames)
run_sim(time, pert, sim_name, "../doc/images/example_02_open_loop_1.png")

# runs a second openloop simulation with a roll/yaw perturbation
pert = np.zeros(dm.sv_size)
pert[dm.sv_beta] += pu.rad_of_deg(10.); pert[dm.sv_phi] += pu.rad_of_deg(-10.)
sim_name = "Roll/Yaw perturbation: {:s}".format(filenames)
run_sim(time, pert, sim_name, "../doc/images/example_02_open_loop_2.png")

plt.show()
